//
// Created by rebeater on 3/23/21.
//
#include <defines.h>
#include "RtTasks.h"
#include <stdio.h>
#include "FreeRTOS.h"
#include<adis16460.h>
#include "cmsis_os.h"
#include "rtcommon.h"

#if ENGINE == PSINS
#include <psins_run.h>
#define EKF_INITIALIZE() kalmanInitialize()
#else
#include <LooselyCouple.h>
//#define EKF_INITIALIZE() kalmanInitialize()
#endif

#include <string.h>
#include <usart.h>
#include "led.h"
#include <fatfs.h>
#include <tim.h>
#include <fdcan.h>
#include <agilex.h>
#include <stim300.h>
#include <rtcommon.h>

#if GNSS_RCV == GNSS_RCV_UB482
#include "GnssParseRaw.h"
#define PARSE_GNSS ParseRawBin
#else
#include "nmea_decode.h"
#define PARSE_GNSS decode_multi_msgs
#endif

int isValid(const GnssData *gnss) {
  if (gnss->mode == RTK_FIX) {
	return 3;
  }
  if (gnss->mode == RTK_FLOAT) {
	return 2;
  }
  if (gnss->mode == RTK_DGPS || gnss->mode == SPP) {
	return 1;
  }
  return 0;
}

FDCAN_RxHeaderTypeDef RxHeader;

extern osMessageQId imu_data_queue_handle;

extern uint32_t system_time_int;
extern uint32_t pps_margin;
extern uint32_t cnt_current;

extern double uart_gpst;
extern osMessageQId gnss_data_queue_handle;
extern osMessageQId imu_data_queue_for_storage_handle;
extern osMessageQId gnss_data_queue_for_storage_handle;/*其他任务发送到存储任务*/
extern osMessageQId rslt_data_queue_for_storage_handle;/*其他任务发送到存储任务*/
extern osMessageQId vel_queue_for_storage_handle;/*其他任务发送到存储任务*/
extern osMessageQId camera_queue_for_storage_handle;/*其他任务发送到存储任务*/
extern osMessageQId nav_queue_for_1_hz_out_handle;
extern osMessageQId vel_queue_handle;

extern osThreadId align_task_handle;
extern osThreadId blink_task_handle;
extern osThreadId timing_handle;
extern osThreadId gnss_task_handle;
extern osThreadId data_storage_task_handle;
extern osThreadId loosely_couple_handle;
extern osThreadId output_task_handle;
extern osThreadId led_task_handle;
extern osThreadId start_task_handle;

extern DateTime date_time;
extern SystemState system_state;

NavOutput navOutput;
Bias bias;
uint8_t flag_sensors = 0x00;
const uint8_t flag_imu = 0x01;
const uint8_t flag_gnss = 0x02;
const uint8_t flag_odo = 0x04;
const uint8_t flag_baro = 0x08;
const uint8_t flag_mag = 0x10;
const uint8_t flag_slam = 0x20;

const float VERSION = 1.03f;

const char brief[] = "*********************************************************\n"
					 "  GNSS/INS Integrated Navigation System based on ARM(R)  \n"
					 "  Version 1.02                                           \n"
					 "  GNSS: MXT906B IMU:ADIS16460                            \n"
					 "  Hardware Version 2.2.1                                 \n"
					 "  Software Version 1.0.3  enable UART 4                  \n"
					 "*********************************************************\n";

uint8_t flag_error = 0;
CameraEvent cameraEvent;
uint8_t camera_buffer[128] ALIGN_RAM_D1;
extern DateTime date_time;
uint8_t imu_status;
float imu_tim2_cnt = 0.0f;
float local_time = 0;

FlagTiming flag_timing = UNTIMING;/*本地授时标志，0 未经授时 1：pps已经到达 2：授时完成*/
uint32_t system_time_int = 0;
double system_time_start = 0;/*系统时间起点，gps*/

SystemState system_state = SYSTEM_START;
uint8_t flag_uart_gnss_ready = 0;
uint8_t flag_align_finished = 0;
uint8_t flag_sdcard_ready = 0;
uint8_t rx_buff[UART_RX_BUFFER_LENGTH] ALIGN_RAM_D1;//__attribute__((section(".Uart2RxSection")));/*GNSS接收数据缓存*/
uint8_t gnss_buff[UART_RX_BUFFER_LENGTH]  ALIGN_RAM_D1;//__attribute__((section(".Uart2GnssSection")));
char kUart6TxBuffer[512] __attribute__((section(".Uart6TxSection")));
uint8_t flag_vel;

void ErrorInStorage() {
  system_state = SYSTEM_ERROR;
  for (;;) { osDelay(1); };
}
#define SD_RESULT_HANDLE(x) \
        if(x!=FR_OK){              \
             trace(Error,"error in sd card %d\n",(int)x) ; \
                ErrorInStorage();\
             }
void show_welcome() {
  HAL_UART_Transmit(&huart6, (uint8_t *)brief, strlen(brief), 0xffff);
}
#if ENGINE == PSINS
void alignIndoor() {
  IMU_RAW raw;
  ImuData imu;
  BaseType_t res;
  kalmanAlignUseDefualt();
  int r;
  do {
	res = xQueueReceive(imu_data_queue_handle, &raw, 0xff);
	if (res != pdTRUE) {
	  trace(Error, "error in AlignMovingTask reading IMU: res = %ld\n", res);
	  continue;
	}
	RAW_TO_IMU(&raw, &imu);
	r = kalmanAlignLevel(&imu);
	trace(Info, "imu gpst = %d\n", (int)(1000 * imu.gpst));
  } while (r == 1);

}
#endif
#if USE_STIM300 == 1
uint8_t rxbuff_stim300[128];
#endif

_Noreturn void mainTask() {
  system_state = SYSTEM_START;
  osThreadDef(blinkTask, blinkTask, osPriorityNormal, 0, 1024);
  blink_task_handle = osThreadCreate(osThread(blinkTask), NULL);
  show_welcome();
  trace(Info,"GnssData %d\n",sizeof(GnssData));
  trace(Info,"ImuData %d\n",sizeof(ImuData));
  trace(Info,"NavOutput %d\n",sizeof(NavOutput));

  ledBlinkTest();
#if USE_STIM300 == 1
  trace(Info, "init STIM300...");
  do {
	  HAL_UART_Receive(&huart4, rxbuff_stim300, 1, 0xFFFF);
	  trace(Info, "stim300 : %d\n", rxbuff_stim300[0]);
  } while (rxbuff_stim300[0] != 0x91);
  __HAL_UART_ENABLE_IT(&huart4, UART_IT_IDLE);
  HAL_UART_Receive_IT(&huart4, rxbuff_stim300 + 1, 27);
#else
  trace(Info, "init adis16460...");
  adis16460_init();
  trace(Info, "OK\n");
  HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);/*开启IO中断检测，此时可以检测IMU和PPS中断*/
  osDelay(10);
#endif
  trace(Info, "detect GNSS receiver......\n");
  __HAL_UART_ENABLE_IT(&huart2, UART_IT_IDLE);
  HAL_UART_Receive_DMA(&huart2, rx_buff, UART_RX_BUFFER_LENGTH);
  osThreadDef(gnssTask, gnssTask, osPriorityNormal, 0, 512);
  gnss_task_handle = osThreadCreate(osThread(gnssTask), NULL);
  system_state = SYSTEM_WAIT_FOR_TIME;
  trace(Info, "wait for TIME...\n");
#if NO_GNSS == 1
  /*无GPS模式启动*/
  system_time_start = 0;
  system_time_int = 0;


  date_time.year = 22;
  date_time.mon = 2;
  date_time.day = 2;
  date_time.second = 2;
  date_time.hour = 22;
  date_time.minute = 22;
  system_time_start = 0;/*记录系统时间起点，本地计时器记录本地时间偏移，pps间隔记录二者比例，即两个pps之间有多少计数*/
  system_time_int = 0;/*对时完成，本地时间清零*/
//    HAL_TIM_Base_Start(&htim2);
  flag_timing = TIMING_OK;
#else /*有GNSS，动态对准*/
  osThreadDef(timingTask, localTimingTask, osPriorityNormal, 0, 512);
  timing_handle = osThreadCreate(osThread(timingTask), NULL);
  while (flag_timing != TIMING_OK) { osDelay(10); };/*等待授时完成*/
  vTaskDelete(timing_handle);/*授时完毕，结束任务*/
#endif
  system_state = SYSTEM_ALIGNING;
  trace(Info, "Start aligning\n");
#if NO_GNSS == 1
  HAL_TIM_Base_Start_IT(&htim2);
  alignIdoor();
#else
  /*TODO double ants*/

  AlignTask();
#endif
  system_state = SYSTEM_NAVIGATION;
  trace(Info, "Start Navigation\n");
  osThreadDef(looselyCouple, LooselyCoupleMainTask, osPriorityHigh, 0, 5120);
  loosely_couple_handle = osThreadCreate(osThread(looselyCouple), NULL);

/*  osThreadDef(outputTask, OutputTask, osPriorityNormal, 0, 256);
  output_task_handle = osThreadCreate(osThread(outputTask), NULL);*/

  /*打开里程计*/
  HAL_FDCAN_Start(&hfdcan1);
//  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
  for (;;) {
	unsigned portBASE_TYPE res1, res2, res3, res4, res5;
	res1 = uxTaskGetStackHighWaterMark(loosely_couple_handle);
	res2 = uxTaskGetStackHighWaterMark(gnss_task_handle);
	res3 = uxTaskGetStackHighWaterMark(data_storage_task_handle);
	res4 = uxTaskGetStackHighWaterMark(NULL);
	res5 = xPortGetFreeHeapSize();
	trace(Info, "%02d:%02d:%02d:  %lu %lu %lu %lu %lu\n", date_time.hour, date_time.minute,
		  date_time.second, res1, res2, res3, res4, res5);
	osDelay(5000);
  }
}

/*本地授时任务，完成任务之后删除*/
_Noreturn void localTimingTask() {
  GnssData gnss_buf;
  BaseType_t res;
  while (1) {
	if (flag_timing != TIMING_OK) {
	  res = xQueueReceive(gnss_data_queue_handle, &gnss_buf, portMAX_DELAY);
	  if (res != pdTRUE && (gnss_buf.week > 0)) {
		printf("error in localTimingTask res = %ld\n", res);
	  } else {
		uart_gpst = gnss_buf.gpst;
		flag_timing = WAIT_FOR_PPS;
		gpst_to_utc(gnss_buf.week, gnss_buf.gpst, &date_time);
	  }
	}
	osDelay(1);
  }
  // 授时完成，开始采集数据和对准
}
uint32_t valid_length;
/*GNSS NMEA解析任务，将数据放入队列，，但是占用也不高*/
_Noreturn void gnssTask() {
  trace(Info, "gnssTask\n");
  BaseType_t res;
  FRESULT fresult;
  GnssData gnss;
  uint32_t data_length = 0;
  UINT len;
  char range_path[128] = "hello world";
  uint8_t flag_raw_file_ready;
  while (1) {
	if (flag_uart_gnss_ready == 1) {
	  flag_uart_gnss_ready = 0;
	  valid_length = PARSE_GNSS((gnss_buff), &gnss);
	  if (valid_length > 0) {
#if DEBUG_INDOOR == 1
		trace(Info, "GNSS: %d MODE: %d YAW: %d Ns:%d\n", (int)gnss.gpst, gnss.mode, (int)gnss.yaw,gnss.ns);
#endif
		res = xQueueSend(gnss_data_queue_handle, &gnss, 0xf);
		res = xQueueSend(gnss_data_queue_for_storage_handle, &gnss, 0xf);
		/* 保存原始数据*/
	  };
	} else { osDelay(1); }

  }
}

/*现在是有返回值，因为作为startDefaultTask的一部分*/
void AlignTask() {
  trace(Info, "AlignTask\n");
  double vel = 0;
  ImuRawAdi adi;
  BaseType_t res;
  GnssData gnss;
  ImuData imu;
  BaseType_t pxHigherPriorityTaskWoken;
  while (pdTRUE) {
	res = xQueueReceive(imu_data_queue_handle, &adi, 0xff);
	if (res != pdTRUE) {
	  trace(Error, "error in AlignMovingTask reading IMU: res = %ld\n", res);
	  continue;
	}
	res = xQueueSend(imu_data_queue_for_storage_handle, &adi, 0x0);
	convert_adi_to_double(&adi, &imu);
	if (kalmanAlignLevel(&imu)) {
	  break;
	};
	res = xQueueReceive(gnss_data_queue_handle, &gnss, 0);
#if ENABLE_DOUBLE_ANTENNA == 1
	if (res == pdTRUE) {
	  /*双天线对准*/
	  if (kalmanAlignGnss(&gnss) > 0) {
		trace(Info, "%d:Double Antenna Align finished\n", (int)gnss.gpst);
//		break;
	  } else {
		trace(Info, "%d:Heading information captured: %d\n", (int)gnss.gpst, (int)(gnss.yaw));
	  }
	}
#else
	if (res == pdTRUE && isValid(&gnss)) {
	  /*动对准*/
	  vel = kalmanAlignPos(&gnss, &imu);
	  trace(Info, "Moving Align:vel = %d mm/s\n", (int)(1000 * vel));
	}
	if (kalmanAlignLevel(&imu)) {
	  flag_align_finished = 1;
	  kalmanOutput(imu.gpst, &navOutput);
	  trace(Info, "align finished heading = %d mdeg\n", (int)(1000 * navOutput.atti[2] / _deg));
	  break;
	}
#endif
  }
  if (kalmanInitialize()) {
	trace(Error, "align failed\n");
  };
  kalmanOutput(&navOutput);
  trace(Info,
		"Initial Position: %d %d %d\n",
		(int)(navOutput.lat),
		(int)(navOutput.lon),
		(int)(navOutput.height));
  trace(Info,
		"Initial Attitude: %d %d %d\n",
		(int)(navOutput.atti[0] / _deg),
		(int)(navOutput.atti[1] / _deg),
		(int)(navOutput.atti[2] / _deg));
  trace(Info,
		"Initial Velocity: %d %d %d\n",
		(int)(navOutput.vn[0] / _deg),
		(int)(navOutput.vn[1] / _deg),
		(int)(navOutput.vn[2] / _deg));
}
GnssMode mode;
uint8_t out_buffer[128] __attribute__((section(".Uart4TxSection")));

_Noreturn void LooselyCoupleMainTask() {
  trace(Info, "LooselyCoupleMainTask\n");
//  while (1) {}
#if ENGINE == PSINS
#else
  IMU_RAW raw;
  ImuData imu;
  Velocity vel;
  BaseType_t res, pxHigherPriorityTaskWoken;
  GnssData gnss;
  double next_gpst = 0;
  int output_length;
//  kalmanInitialize();
//trace(Info,"tp0 time = %d ms\n",(int)(1000*imu.gpst));
  while (pdTRUE) {
	res = xQueueReceive(imu_data_queue_handle, &raw, 0xff);
	if (res != pdTRUE) {
	  continue;
	}
//	trace(Info,"tp1 time = %d ms\n",(int)(1000*raw.time_s));
	flag_sensors |= flag_imu;
	res = xQueueSend(imu_data_queue_for_storage_handle, &raw, 0xf);
	if (res != pdTRUE) {
	  //*意味着速度不够*//*
	  flag_error = 1;
	}
//	trace(Info,"tp3 time = %d ms\n",(int)(1000*raw.time_s));
	res = xQueueReceive(gnss_data_queue_handle, &gnss, 0);
	if (res == pdTRUE) {
//	  trace(Info, "Update: %d MODE: %d YAW: %d Ns:%d\n", (int)gnss.gpst, gnss.mode, (int)gnss.yaw,gnss.ns);
	  mode = gnss.mode;
	  flag_sensors |= flag_gnss;
	  if (isValid(&gnss) > 0) {
		kalmanSetGNSS(&gnss);/*TODO 延迟更新的处理方案还没做*/
#if 0 /*打印部分调试信息*/
		trace(Info,
			  "gnss update,%d %d %d %d  %d/%d/%d %d\n",
			  (int)gnss.gpst,
			  (int)gnss.lat,
			  (int)gnss.lon,
			  (int)gnss.ns,
			  (int)(100 * gnss.pos_std[0]),
			  (int)(100 * gnss.pos_std[1]),
			  (int)(100 * gnss.pos_std[2]),
			  gnss.mode);
				double xd[STATE_CNT];
				getXd(xd);
				char double_buffer[128];
				for (int i = 0; i < STATE_CNT; i++) {
				  sprintDouble(double_buffer, xd[i], 3);
				  HAL_UART_Transmit(&huart6, double_buffer, strlen(double_buffer), 0xff);
				}
				trace(Info, "\n");
#endif
	  }
	}
	res = xQueueReceive(vel_queue_handle, &vel, 0);
	if (res == pdTRUE) {
	  flag_sensors |= flag_odo;
	  kalmanSetVel(&vel);
	  xQueueSend(vel_queue_for_storage_handle, &vel, 0xf);
	}
	RAW_TO_IMU(&raw, &imu);
	kalmanUpdate(&imu);
	kalmanOutput(&navOutput);
	navOutput.info.gnss_mode = mode;
	navOutput.info.sensors = flag_sensors;
	/*! 主输出端口，输出高频组合导航消息，建议采用DMA,打开一路超高速串口*/
	output_length = agilex_encode(out_buffer, &navOutput);
	HAL_UART_Transmit_DMA(&huart4, out_buffer, sizeof(NavOutput)+6);
	if (res != pdTRUE) {
	  flag_error = 1;
	}
	if (imu.gpst > next_gpst) {
	  next_gpst = (int)imu.gpst + 1.0;
	  sprintOutput(kUart6TxBuffer, &navOutput);
	  SCB_CleanInvalidateDCache_by_Addr((uint32_t *)kUart6TxBuffer, 512);
	  HAL_UART_Transmit_DMA(&huart6, (uint8_t *)kUart6TxBuffer, strlen(kUart6TxBuffer));
	}
  }
#endif
}

IMU_RAW raws[32] ALIGN_RAM_D1 /*__attribute__((section(".ImuBuffer1024"))) */  ;
NavOutput outs[32] ALIGN_RAM_D1/*__attribute__((section(".RstBuffer1024"))) */ ;
Velocity vels[32] ALIGN_RAM_D1 /*__attribute__((section(".VelBuffer1024")))*/ ;
GnssData gnss ALIGN_RAM_D1 /*__attribute__((section(".GNSSBuffer1024")))*/ ;
#if ENABLE_CAMERA == 1
CameraEvent events[32] ALIGN_RAM_D1;
#endif
_Noreturn void SDCardStorageTask() {
  trace(Info, "task SDCardStorageTask\n");
  int idx_imu = 0, idx_rslt = 0, idx_vel = 0, idx_ent = 0, raw_length = 0;
  uint8_t idx_gnss = 0;
  FRESULT res, res1, res2;
  char imu_filename[25], gnss_filename[25], rslt_filename[25], vel_filename[25], event_filename[25], raw_filename[25];
  /*! d等待授时完成之后开始写入数据*/
  while (flag_timing != TIMING_OK) { osDelay(10); }
  double gpst = systemTimeToGpst(system_time_int, htim2.Instance->CNT, pps_margin);
  /*! 挂载文件系统，打开文件存储数据*/
  res = f_mount(&SDFatFS, SDPath, 1);
  if (res == FR_OK) {
	trace(Info, "mount TF card OK\n");
  } else {
	trace(Info, "mount TF card  failed...%d\n", res);
	ErrorInStorage();
  }
  flag_sdcard_ready = 1;
#if NO_GNSS == 1
  int file_index_1 = 1, file_index_2 = 1, file_index_3 = 1;
  FRESULT res3, res4;
  do {
	  sprintf(imu_filename, "INDOOR_ADI_%06d.adi", file_index_1++);
	  sprintf(gnss_filename, "INDOOR_GNSS_%06d.gps", file_index_2++);
	  sprintf(rslt_filename, "INDOOR_RSLT_%06d.rst", file_index_3++);
	  sprintf(vel_filename, "INDOOR_VEL_%06d.vel", file_index_3++);
	  res1 = f_open(&IMUFile, imu_filename, FA_READ);
	  f_close(&IMUFile);
	  res2 = f_open(&GNSSFile, gnss_filename, FA_READ);
	  f_close(&GNSSFile);
	  res3 = f_open(&RsltFile, rslt_filename, FA_READ);
	  f_close(&RsltFile);
	  res4 = f_open(&VelFile, vel_filename, FA_READ);
	  f_close(&VelFile);
  } while (res1 == FR_OK && res2 == FR_OK && res3 == FR_OK && res4 == FR_OK);
#else
  sprintf(imu_filename, "ADI_%02d%02d%02d_%06d.adi", date_time.year - 20, date_time.mon, date_time.day, (int)gpst);
  sprintf(event_filename, "EVENT_%02d%02d%02d_%06d.ent", date_time.year - 20, date_time.mon, date_time.day, (int)gpst);
  sprintf(gnss_filename, "GNSS_%02d%02d%02d_%06d.gps", date_time.year - 20, date_time.mon, date_time.day, (int)gpst);
  sprintf(rslt_filename, "RSLT_%02d%02d%02d_%06d.rst", date_time.year - 20, date_time.mon, date_time.day, (int)gpst);
  sprintf(vel_filename, "VEL_%02d%02d%02d_%06d.vel", date_time.year - 20, date_time.mon, date_time.day, (int)gpst);
  sprintf(raw_filename, "RAW_%02d%02d%02d_%06d.raw", date_time.year - 20, date_time.mon, date_time.day, (int)gpst);
#endif
  UINT len;
  res = f_open(&IMUFile, imu_filename, FA_CREATE_ALWAYS | FA_WRITE);
  SD_RESULT_HANDLE(res);
  res = f_open(&GNSSFile, gnss_filename, FA_CREATE_ALWAYS | FA_WRITE);
  SD_RESULT_HANDLE(res);
  res = f_open(&RsltFile, rslt_filename, FA_CREATE_ALWAYS | FA_WRITE);
  SD_RESULT_HANDLE(res);
  res = f_open(&VelFile, vel_filename, FA_CREATE_ALWAYS | FA_WRITE);
  SD_RESULT_HANDLE(res);
  res = f_open(&GNSSRawFile, raw_filename, FA_CREATE_ALWAYS | FA_WRITE);
  SD_RESULT_HANDLE(res);
#if ENABLE_CAMERA == 1
  res = f_open(&EntFile, event_filename, FA_CREATE_ALWAYS | FA_WRITE);
  SD_RESULT_HANDLE(res);
#endif
  trace(Info, "Create file %s %s %s\n", imu_filename, gnss_filename, rslt_filename);
  /* Infinite loop */
  for (;;) {
	/* Info */
	if (idx_imu >= 32) {
	  idx_imu = 0;
	  res1 = f_write(&IMUFile, raws, sizeof(IMU_RAW) * 32, &len);
	  res2 = f_sync(&IMUFile);
	  if (res1 == FR_OK && res2 == FR_OK) {
		/*  printf("write imu %d bytes\n ", len);*/
	  } else {
		trace(Error, "write imu failed res1 = %d res2 = %d\n", res1, res2);
		ErrorInStorage();
	  }
	}
	if (valid_length > 0) {
	  res = f_write(&GNSSRawFile, gnss_buff, valid_length, &len);
	  raw_length += valid_length;
	  if (raw_length > 4096) {
		f_sync(&GNSSRawFile);
		raw_length = 0;
	  }
	  valid_length = 0;
	}

	if (idx_rslt >= 32) {
	  idx_rslt = 0;
	  res1 = f_write(&RsltFile, outs, sizeof(NavOutput) * 32, &len);
	  res2 = f_sync(&RsltFile);
	  if (res1 == FR_OK && res2 == FR_OK) {
		/* printf("write rslt %d bytes\n ", len);*/
	  } else {
		trace(Error, "write rslt failed res1 = %d res2 = %d\n", res1, res2);
		flag_error = 1;
	  }
	}//*/

	if (idx_vel >= 32) {
	  idx_vel = 0;
	  res1 = f_write(&VelFile, vels, sizeof(Velocity) * 32, &len);
	  res2 = f_sync(&VelFile);
	  if (res1 == FR_OK && res2 == FR_OK) {
		/* printf("write rslt %d bytes\n ", len);*/
	  } else {
		trace(Error, "write VelFile failed res1 = %d res2 = %d\n", res1, res2);
		flag_error = 1;
		ErrorInStorage();
	  }
	}//*/
#if ENABLE_CAMERA == 1
	if (idx_ent >= 32) {
	  idx_ent = 0;
	  res1 = f_write(&EntFile, events, sizeof(CameraEvent) * 32, &len);
	  res2 = f_sync(&EntFile);
	  if (res1 == FR_OK && res2 == FR_OK) {
		trace(Info, "write event file %d bytes\n", len);
	  } else {
		trace(Error, "write EntFile failed res1 = %d res2 = %d\n", res1, res2);
		flag_error = 1;
		ErrorInStorage();
	  }
	}//*/
#endif
	res = xQueueReceive(gnss_data_queue_for_storage_handle, &gnss, 0);
	if (res == pdTRUE) {
	  res1 = f_write(&GNSSFile, &gnss, sizeof(GnssData), &len);
	  if (res1 == FR_OK) {
	  } else {
		trace(Error, "write rslt failed res1 = %d res2 = %d\n", res1, res2);
		ErrorInStorage();
	  }
	  idx_gnss++;
	  if (idx_gnss >= 8) {
		res2 = f_sync(&GNSSFile);
		idx_gnss = 0;
	  }
	}
	res = xQueueReceive(imu_data_queue_for_storage_handle, &raws[idx_imu], 0);
	if (res == pdTRUE) {
	  idx_imu++;
	}
	res = xQueueReceive(vel_queue_for_storage_handle, &vels[idx_vel], 0);
	if (res == pdTRUE) {
	  idx_vel++;
	}
	res = xQueueReceive(rslt_data_queue_for_storage_handle, &outs[idx_rslt], 0);
	if (res == pdTRUE) {
	  idx_rslt++;
	}//*/
#if ENABLE_CAMERA == 1
	res = xQueueReceive(camera_queue_for_storage_handle, &events[idx_ent], 0);
	if (res == pdTRUE) {
	  idx_ent++;
	}//*/
#endif
//        osDelay(1);
  }
}

/**
 * 数据输出任务，fasdf2
 */
_Noreturn void OutputTask() {
  trace(Info, "Start output\n");
  BaseType_t res;
  NavOutput out;
  while (pdTRUE) {
	trace(Info, "Into while\n");
	res = xQueueReceive(nav_queue_for_1_hz_out_handle, &out, 1);
	trace(Info, "After while\n");
	if (res != pdTRUE) {
	  trace(Error, "OutputTask:xQueueReceive taking failed\n");
	  continue;
	}
//	trace(Info,"%d %d %d\n",(int)out.gpst,(int)(out.pos[0]/_deg),(int)(out.pos[1]/_deg));
	sprintOutput(kUart6TxBuffer, &out);
	SCB_CleanInvalidateDCache_by_Addr((uint32_t *)kUart6TxBuffer, 512);
	HAL_UART_Transmit_DMA(&huart6, (uint8_t *)kUart6TxBuffer, strlen(kUart6TxBuffer));
  }
}

/**
 * LED 任务 闪灯控制
 * @param c 颜色
 * @param up 亮灯时间
 * @param down 灭灯时间
 */
void blink(LedColor c, int up, int down) {
  setLed(c);
  osDelay(up);
  setLed(LED_OFF);
  osDelay(down);
}

_Noreturn void blinkTask() {
  while (1) {
	switch (system_state) {
	  case SYSTEM_START:setLed(LED_RED);
		osDelay(1);
		break;
	  case SYSTEM_WAIT_FOR_TIME:blink(LED_RED, 500, 500);/*修改为红色增加区分度*/
		break;
	  case SYSTEM_ALIGNING:blink(LED_YELLOW, 200, 300);
		break;
	  case SYSTEM_NAVIGATION:
		switch (mode) {
		  case RTK_DGPS:setLed(LED_BLUE);
			break;
		  case RTK_FLOAT:setLed(LED_BLUE);
			break;
		  case RTK_FIX:setLed(LED_GREEN);
			break;
		  case SPP:setLed(LED_CYAN);
			break;
		  case INITIAL:
		  case UNKNOWN_PPS:
		  case INVALID:setLed(LED_PINK);
			break;
		  default:setLed(LED_PINK);
			break;
		}
		osDelay(400);
		setLed(LED_OFF);
		osDelay(1600);
		break;
	  case SYSTEM_ERROR:blink(LED_RED, 100, 100);
		break;
	  default:break;
	}
  }
}

uint32_t pps_margin = 1e6;/*两个PPS之间的间隔，用于修正本地时间 */
uint64_t cnt_on_pps; /*CNT计数*/
uint32_t cnt_current;
double uart_gpst = 0;/*gnss time 上时uart的GPS时间*/
extern uint8_t flag_camera_event_ready;
extern CameraEvent cameraEvent;
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
  if (GPIO_Pin == CAM_INT_Pin) {
	cameraEvent.time_s = system_time_start + (system_time_int) + htim2.Instance->CNT * 1.0 / pps_margin;/**/
	flag_camera_event_ready = 1;
	return;
  }

  if (GPIO_Pin == ADI_INT_Pin) { /** ADI 惯导数据ready */
#if USE_STIM300 == 0
	if (flag_timing != TIMING_OK) {
	  return;
	}
	local_time += 1;
	ImuRawAdi adi;
	BaseType_t res, px_higher_priority_task_woken;
	/** tims_s = gpst0 + (1e6*timer_cnt + cnt)/pps_margin
	 * */
	adi.time_s = system_time_start + (system_time_int) + htim2.Instance->CNT * 1.0 / pps_margin;/**/
	adis16460_burst_read(&adi);
	res = xQueueSendFromISR(imu_data_queue_handle, &adi, &px_higher_priority_task_woken);
	if (res != pdTRUE) {
	  /*意味着速度不够*/
//            trace(Info,"Error in HAL_GPIO_EXTI_Callback %ld, Is there too much data in the imuDataQueueHandle ? \n", res);
//            Error_Handler();
//            for(;;) {};
	}
#endif
  }
  if (GPIO_Pin == GNSS_PPS_Pin) {/** GNSS PPS ready*/
	if (flag_timing == WAIT_FOR_PPS) { /*本地时间校准*/
	  HAL_TIM_Base_Start_IT(&htim2);/*开启本地时间计时器*/
	  system_time_start = uart_gpst + 1.0;/*记录系统时间起点，本地计时器记录本地时间偏移，pps间隔记录二者比例，即两个pps之间有多少计数*/
	  system_time_int = 0;/*对时完成，本地时间清零*/
	  flag_timing = TIMING_OK;
	  /*    printf("timing finished\n");*/
	}
	cnt_current = htim2.Instance->CNT;
	pps_margin = (uint64_t)1e6 * system_time_int + cnt_current - cnt_on_pps;
	cnt_on_pps = (uint64_t)1e6 * system_time_int + cnt_current;
	if (pps_margin > 1000000 + 20 || pps_margin < 1000000 - 20) {/*避免pps中断造成margin计算异常*/
	  pps_margin = 1e6;
	}
	htim2.Instance->ARR = pps_margin - 1;
  }
}
uint8_t can1RxData[8];
void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t rx_fifo_0_i_ts) {
  Velocity vel;
  if (hfdcan->Instance == FDCAN1) {
	BaseType_t res, px_higher_priority_task_woken;
	HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO0, &RxHeader, can1RxData);
	if (RxHeader.Identifier == AGILEX_VELOCITY_IDENTIFIER) {
	  /*速度判定*/
	  agilexGetVelocity(&vel, &RxHeader, can1RxData);
	  vel.time_s = system_time_start + (system_time_int) + htim2.Instance->CNT * 1.0 / pps_margin;/**/
	  res = xQueueSendFromISR(vel_queue_handle, &vel, &px_higher_priority_task_woken);
	  if (res != pdTRUE) {
		/*意味着速度不够*/
	  }
	}
  }
  if ((rx_fifo_0_i_ts & FDCAN_IT_RX_FIFO0_WATERMARK) != RESET) {
  }
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
  printf("data receive\n");
}
